% the main simulation file (run this)
% setup the robot in robotSetup.m
clear;
tic;
robotSetup;
stateDfn;

% ROBOT INITIAL STATE 
x0(SX) = 0;
x0(SY) = 0;
x0(STH) = 0;
x0(SDX) = 0;
x0(SDY) = 0;
x0(SDTH) = 0;
% modules (2)
for i=1:robot.modules
	x0(SPHI + modS*(i-1)) = 0;
	x0(SDPHI + modS*(i-1)) = 0;
	x0(SI + modS*(i-1)) = 0;
	x0(SG + modS*(i-1)) = 0;
end
x0 = x0'; % transpose (required)

% Simulation Parameters
t = 0:0.005:15;

% simulate!
[t,x] = portableODESolver('swerveEOM', t, x0);

% plot some results
figure(1);
plot(x(:,SX), x(:,SY));
xlabel('X position (m)');
ylabel('Y position (m)');
title('Robot Path');

figure(3);
plot(t, x(:,SX));
title('Robot X-position vs. Time');

figure(4);
plot(t, x(:,SDX));
title('Robot X-velocity vs. Time');

figure(6);
plot(t, x(:,SY));
title('Robot Y-position vs. Time');

figure(7);
plot(t, x(:,SDY));
title('Robot Y-velocity vs. Time');

speeds = zeros(length(t),robot.modules);
for i = 1:length(t)
	speeds(i,:) = wheelSpeeds(x(i,:), t(i), robot);
end
figure(8);
plot(t, speeds(:,1), t, speeds(:,2));
title('Wheel1 Speed vs. Time');
legend('Wheel 1', 'Wheel 2');

%figure(5);
%plot(t, x(:,STH));
%title('Robot Angle vs. Time');


% figure(2);
% hold on;
%for i = 1:robot.modules
%	plot(t, x(:,SPHI+(i-1)*modS))
%end
%hold off;
%xlabel('Time, t (sec)');
%ylabel('Module Angle, \phi_i (rad)');
% TODO legend (auto-scale to module qty)

toc;
